'''
####
Date : 2024-10-17
Notes: 示教器server
Test: 通讯格式
1. 修改代码执行部分代码---解析代码长度(通讯接收缓存)
    client_socket.recv(10240, 0x40),第二个参数 阻塞/与否
2. 增添视觉通讯功能代码
3. KBot通讯功能完善
4. 0.1.0版本--2024.10.29
####

Modify: 
1. kata新库更换--详见self.robot_control类


####
'''

# -*- coding: utf-8 -*-
import logging
import os
import sys
import socket
import struct
import time
import signal
import modbus_tk.defines as cst
import modbus_tk.modbus_tcp as modbus_tcp
import psutil

from threading import Thread
from PyQt5 import uic, QtCore
from PyQt5.QtWidgets import QApplication, QMainWindow, QVBoxLayout, QTableWidget, QTableWidgetItem, QHBoxLayout, QSizePolicy, QHeaderView
from PyQt5.QtGui import QFont
from PyQt5.QtCore import Qt, QTimer

from processModbusTCP__ import process_Master
from processModbusRTU_ import rtu_Master
from processCommunicate_ import VSR_Server

### 日志文件
robot_logger=logging.getLogger("ZMove")
robot_logger.setLevel(logging.DEBUG)
### 默认显示
os.environ['DISPLAY'] = ':0'


class Controler_Server():

    def __init__(self, host = "0.0.0.0", port = 502):
        super().__init__()
        host_name = socket.gethostname()
        self.host_add = socket.gethostbyname(host_name)
        self.host = host
        self.port = port

        self.server_tcp_init()
        self.vsrserver = VSR_Server()
        self.server_th = Thread(target= self.vsrserver.run)
        self.server_th.start()

        self.th_update = Thread(target= self.status_update)
        self.th_update.start()

    def server_tcp_init(self,):
        try:
            self.server = modbus_tcp.TcpServer(port=self.port, address=self.host, timeout_in_sec=5)
            self.server.start()
            self.register_init()
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def check_port_conflict(self,):
        try:
            ret = False
            sock = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
            result = sock.connect_ex(self.host,self.port) # 检测有没有服务将这个端口以及IP地址占用
            if result == 0:
                ret = True
            else:
                ret = False
            sock.close()
            return ret
        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    def register_init(self,):
        self.slave =self.server.add_slave(1) 
        self.slave.add_block("robot6",cst.HOLDING_REGISTERS, 0, 600) 
        self.slave.add_block("var", cst.COILS, 0, 600)

    def int2float(self,a,b):
        f=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            f=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
        except BaseException as e:
            print(e)
        return f

    def float_to_int16s(self,f):
        # 将浮点数打包成32位二进制数据
        b = struct.pack('f', f)
        # 将32位二进制数据拆分成两个16位二进制数据
        i1, i2 = struct.unpack('HH', b)
        return i2, i1

    def status_update(self,):
        while True:
            try:
                time.sleep(0.005)
                ### 更新机器人的状态
                status = []
                DI_status = []
                DO_status = []
                slider_status = []
                
                s = self.vsrserver.robot_control.status_global
                if s != None:
                    if s["state"] == "Idle":
                        status.append(1)
                    elif s["state"] == "Alarm":
                        status.append(2)
                    elif s["state"] == "Home":
                        status.append(3)
                    elif s["state"] == "Run":
                        status.append(4)
                    else:
                        status.append(0)
                    ang = s["angle"]
                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status.append(a[0])
                        status.append(a[1])

                    ang = s["coordinate"]

                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status.append(a[0])
                        status.append(a[1])
                        if i ==6:
                            slider_status.append(a[0])
                            slider_status.append(a[1])
                    
                    ### 增加机器人的类型
                    status.append(self.vsrserver.robot_control.robot_type_number)     
                    ### 将机器人的状态更新到0号寄存器
                    self.slave.set_values("robot6", 0, status)
                    
                    ###DO 9个的值写入28---36寄存器地址
                    DO_Status = rtu_master.rtu_server.execute(slave=1, function_code=cst.READ_HOLDING_REGISTERS, starting_address=11, quantity_of_x= 9)
                    if DO_Status is not None and len(DO_Status) == 9:
                        for i in range(len(DO_Status)):
                            DO_status.append(DO_Status[i])
                            self.slave.set_values("robot6", 28, DO_status)
                    else:
                        # 如果获取的数据有问题，使用默认初始化值udo
                        DO_status.append([0] * 9)
                        self.slave.set_values("robot6", 28, DO_status)
                    
                    time.sleep(0.5)

                    DI_Status = rtu_master.rtu_server.execute(slave=1, function_code=cst.READ_HOLDING_REGISTERS, starting_address=0, quantity_of_x= 11)

                    if DI_Status is not None and len(DI_Status) == 11:
                        for i in range(len(DI_Status)):
                            DI_status.append(DI_Status[i])
                        self.slave.set_values("robot6", 37, DI_status)
                    else:
                        # 如果获取的数据有问题，使用默认初始化值
                        DI_status.append([0] * 11)
                        self.slave.set_values("robot6", 37, DI_status)
                    time.sleep(1)
                    
            
                ### 更新机器人2的状态
                status_2 = []
                slider_status_2 = []
                s_2 = self.vsrserver.robot_control.status_global_
                if s_2 != None:
                    if s_2["state"] == "Idle":
                        status_2.append(1)
                    elif s_2["state"] == "Alarm":
                        status_2.append(2)
                    elif s_2["state"] == "Home":
                        status_2.append(3)
                    elif s_2["state"] == "Run":
                        status_2.append(4)
                    else:
                        status_2.append(0)
                    ang = s_2["angle"]
                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status_2.append(a[0])
                        status_2.append(a[1])

                    coor = s_2["coordinate"]
                    for i in range(len(coor)):
                        a = self.float_to_int16s(coor[i])
                        status_2.append(a[0])
                        status_2.append(a[1])
                        if i ==6:
                            slider_status_2.append(a[0])
                            slider_status_2.append(a[1])
                    
                    ### 增加机器人的类型
                    status_2.append(self.vsrserver.robot_control.robot_type_number_2)     
                    # ### 增加数字仿真服务状态
                    # status_2.append(self.vsrserver.connect_flag)
                    ### IO状态
                    ### 将2号机器人的状态更新到100号寄存器
                    self.slave.set_values("robot6", 60, status_2)


            except Exception as e:
                robot_logger.error(e,exc_info=True)

    def dorun(self,):
        while True:
            try:
                time.sleep(0.1)
                cmd_di = self.slave.get_values("robot6", 73,2)
                if cmd_di[0] == 0:
                    pass
                elif cmd_di[0] == 1:
                    self.vsrserver.robot_control.robot_stop(com_Num = 1)
                    self.vsrserver.robot_control.robot_stop(com_Num = 2)
                    self.slave.set_values("robot6", 73, 0)
                
                if cmd_di[1] == 0:
                    pass
                elif cmd_di[1] == 1:
                    self.vsrserver.robot_control.robot_home(com_Num=1)
                    self.vsrserver.robot_control.robot_home(com_Num=2)
                    self.slave.set_values("robot6", 74, 0)


                ### 3执行机器人的指令
                time.sleep(0.05)
                cmd = self.slave.get_values("robot6", 99, 1)

                ### 3 Stop 1 Home 2 Zero
                if cmd[0] == 0:
                    pass
                elif cmd[0] == 1:
                    self.vsrserver.robot_control.home_position(com_Num=1)
                    self.vsrserver.robot_control.home_position(com_Num=2)
                    self.slave.set_values("robot6", 99, 0)
                elif cmd[0] == 2:
                    self.vsrserver.robot_control.go_to_zero()
                elif cmd[0] == 3:
                    self.vsrserver.robot_control.robot_stop(com_Num=1)
                    self.vsrserver.robot_control.robot_stop(com_Num=2)

                ### 4 控制台执行机器人的指令
                mini_factroy_cmd = self.slave.get_values("robot6", 37, 2)
                # mini_factroy_cmd = rtu_master.rtu_server.execute(1, cst.READ_HOLDING_REGISTERS, starting_address= 0, quantity_of_x=2)
                if mini_factroy_cmd[0] == 0 :
                    pass
                elif mini_factroy_cmd[0] == 1:
                    self.vsrserver.robot_control.robot_stop(com_Num=1)
                    self.vsrserver.robot_control.robot_stop(com_Num=2)
                
                if mini_factroy_cmd[1] == 0:
                    pass
                elif mini_factroy_cmd[1] == 1:
                    self.vsrserver.robot_control.home_position(com_Num=1)
                    self.vsrserver.robot_control.home_position(com_Num=2)
            
                ### 5Lua脚本的指令
                time.sleep(0.05)
                cmd = self.slave.get_values("robot6", 98, 1)

                ### 1.runLua 2. stopLua 3. terminalLua
                if cmd[0] == 0:
                    pass
                elif cmd[0] == 1:
                    self.vsrserver.lua_robotLua.runningLua = True
                    self.slave.set_values("robot6", 98, 0)
                elif cmd[0] == 2:
                    self.vsrserver.robot_control.allRobot_stop()            
                    self.vsrserver.lua_robotLua.stop_process()
                    rtu_master.rtu_server.execute(1, cst.WRITE_MULTIPLE_REGISTERS, starting_address=10, output_value=[0,0,0,0,0,0,0,0])
                    self.slave.set_values("robot6", 98, 0)
                elif cmd[0] == 3:
                    self.vsrserver.robot_control.allRobot_stop()
                    self.vsrserver.lua_robotLua.stop_process()
                    rtu_master.rtu_server.execute(1, cst.WRITE_MULTIPLE_REGISTERS, starting_address=10, output_value=[0,0,0,0,0,0,0,0])
                    self.slave.set_values("robot6", 98, 0)
            
            except Exception as e:
                robot_logger.error(e,exc_info=True)



class MainWindow(QMainWindow):
    
    def __init__(self):
        super().__init__()
        self.initParameters()
        self.setup_ui()
        self.init_pushButtonSignals()
        self.init_labelSingls()
        self.set_initial_size()
        self.robot_control_thread()
        
        self.robotInfo_thread = Thread(target=self.init_labelRobotAB)
        self.robotInfo_thread.start()

        #### 变量监控页面显示
        self.tables = []
        self.boolMonitor()
        self.intMonitor()
        self.floatMonitor()
        self.poseMonitor()

    ### 初始化参数
    def initParameters(self):
        self.robotA_type = None
        self.robotB_type = None
        ### 机器人运动步进、速度
        self.step = 5
        self.speed = 2000
    
    ### 初始化UI显示位置及尺寸
    def set_initial_size(self):
        self.setGeometry(0, 0, 1280, 600)
    
    ### 加载qt designer文件
    def setup_ui(self):
        current_dir = os.path.dirname(os.path.abspath(__file__))
        ui_file_path = os.path.join(current_dir, "..", "ui", "mainWindow.ui")
        uic.loadUi(ui_file_path, self)

    ### 初始化UI按钮信号
    def init_pushButtonSignals(self):
        try:
            # Mapping buttons to their corresponding stacked widget indices
            button_to_page = {
                self.pushButton_tpu: 1,
                self.pushButton_monitor: 2,
                self.pushButton_vision: 3,
                self.pushButton_tcp: 4,
                self.pushButton_comeBack: 0,
                self.pushButton_comeBack_2: 0,
                self.pushButton_comeBack_3: 0,
                self.pushButton_comeBack_4: 0,

            }

            for button, page_index in button_to_page.items():
                button.clicked.connect(lambda checked, index=page_index: self.stackedWidget.setCurrentIndex(index))

            robot_button_to_page = {
                self.pushButton_pageArobot: 0,
                self.pushButton_pageBrobot: 1,
            }

            for butt, page_ind in robot_button_to_page.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.stackedWidget_robot.setCurrentIndex(index))
            
            self.pushButton_close.clicked.connect(self.close)
            
            self.mointor_pushButton()

        except Exception as e:
            print(f"init_pushButtonSingnals error is {e}")

    ### 机器人控制 示教--复位--回零--急停
    def robot_control_thread(self):
        self.robotA_jointTeach()
        self.robotA_coordinateTeach()
        self.robotB_jointTeach()
        self.robotB_coordinateTeach()
        self.cmd_robot()

    ### 初始化UI标签显示
    def init_labelSingls(self):
        self.label_IPaddress.setText(self.get_local_ip())
        self.label_IPaddress_2.setText(self.get_local_ip())
    
    ### 获取本地IP地址
    def get_local_ip(self):
        for interface, addrs in psutil.net_if_addrs().items():
            for addr in addrs:
                if addr.family == socket.AF_INET and not addr.address.startswith("127."):
                    return addr.address
        return None

    '''
    示例代码：
        1. UI数据绑定
        2. 示教控制代码:412行 手动控制界面-----Start  ---->  887 行 手动控制界面-----End
    '''
    ### 数值转换函数--读取寄存器值，转换为float用于显示
    def int2float(self,a,b):
        value=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            value=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
            if value is None:
                return value
            if isinstance(value, float):
                # 精确到小数点后两位数
                return round(value , 2)
            else:
                return value
        except BaseException as e:
            print(e)
        return value

#############################
####### 手动控制界面-----Start
    ###-- 1. 机器人类型：KT-R6（6轴机器人）， KT-R4
    ###-- 2. 机器人状态： 离线--空闲-- 锁定--复位--运行
    def init_labelRobotAB(self):
        try:
            while True:
                self.robotA_type = server.vsrserver.get_robot_type("robot_type")
                self.robotB_type = server.vsrserver.get_robot_type("robot_type2")
                self.label_robotAtype.setText(self.robotA_type)
                self.label_robotBtype.setText(self.robotB_type)

                state_val = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, 0, 1)[0]
                if state_val == 0:
                    robotA_state = "离线"
                elif state_val == 1:
                    robotA_state = "空闲"
                elif state_val == 2:
                    robotA_state = "锁定"
                elif state_val == 3:
                    robotA_state = "复位"
                elif state_val == 4:
                    robotA_state = "运行"
                else:
                    robotA_state = "未知"
                
                state_val = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, 0, 1)[0]
                if state_val == 0:
                    robotB_state = "离线"
                elif state_val == 1:
                    robotB_state = "空闲"
                elif state_val == 2:
                    robotB_state = "锁定"
                elif state_val == 3:
                    robotB_state = "复位"
                elif state_val == 4:
                    robotB_state = "运行"
                self.label_robotAstatus.setText(robotA_state)
                self.label_robotBstatus.setText(robotB_state)

                self.joint_coordinateShow()

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    ###-- 3. 机器人坐标轴角、笛卡尔显示： joint_coordinateShow
    ###-- 4. 外层函数_get_robot_pose， 不同机器人取地址不同get_pose_floats， 寄存器读取姿态：read_pose_floats
    def read_pose_floats(self, starting_address):
        motion = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address, 26)
        if not motion:
            return []

        floats = []
        for i in range(0, 26, 2):
            if i + 1 < len(motion):
                floats.append(self.int2float(motion[i], motion[i+1]))
            else:
                print("Error: Not enough integers to convert to floats.")
                break
        return floats

    def get_pose_floats(self, robotNum):
        try:
            if robotNum == 1:
                robot1_pose = self.read_pose_floats(1)
                return robot1_pose
            elif robotNum == 2:
                robot2_pose = self.read_pose_floats(61)
                return robot2_pose

        except Exception as e:
            print(e)

    def _get_robot_pose(self, robot_id):  
        try:  
            pose = self.get_pose_floats(robot_id)  
            return pose 
         
        except Exception as e:  
            return f"Error: {str(e)}"  

    def joint_coordinateShow(self):
        try:
            robot_pose = self._get_robot_pose(1)
            joints = robot_pose[:6]    
            coords = robot_pose[6:12]  
    
            for i, joint in enumerate(joints, 1):  
                getattr(self, f'label_J{i}show').setText(str(joint))
            for i, coord in enumerate(coords, 1):
                getattr(self, f'label_C{i}show').setText(str(coord))
            
            # # 如果机器人有第7个关节 
            # if len(robot_pose) > 12:  
            #     getattr(self, f'label_J7').setText(str(robot_pose[12]))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    ###-- 5. A机器人控制：robotA_jointTeach轴角   robotA_coordinateTeach笛卡尔示教
    def robotA_jointTeach(self):
        try:
            joint_Pushbutton = {
                self.pushButton_J1min: 1,
                self.pushButton_J2min: 2,
                self.pushButton_J3min: 3,
                self.pushButton_J4min: 4,
                self.pushButton_J5min: 5,
                self.pushButton_J6min: 6,
                self.pushButton_J1add: 7,
                self.pushButton_J2add: 8,
                self.pushButton_J3add: 9,
                self.pushButton_J4add: 10,
                self.pushButton_J5add: 11,
                self.pushButton_J6add: 12,
            }
            
            # for butt, page_ind in joint_Pushbutton.items():
            #     butt.clicked.connect(lambda checked, index=page_ind: self.jointControRobotA(index))
            for butt, page_ind in joint_Pushbutton.items():
                # 连接 pressed 信号到第一个函数
                butt.pressed.connect(lambda checked=False, index=page_ind: self.jointControRobotA(index))
                # 连接 released 信号到第二个函数
                butt.released.connect(lambda checked=False, index=page_ind: self.jointControRobotRelease(index))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    def jointControRobotA(self,index):
        try:
            speed = 2000
            s_data = server.vsrserver.robot_control.status_global
            print(f"s_data {s_data}")
            angle_current = s_data["angle"]
            angle_current = [int(i) for i in angle_current]
            if self.robotA_type == "KT-R6":
                ### 1 2 3 4 5 6 轴角 减  7 8 9 10 11 12 轴角 加  21 复位  21 回零 23 急停
                if index == 1:
                    gcode = "M21 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 2:
                    gcode = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 3:
                    gcode = "M21 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 4:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[4],angle_current[5], speed)
                elif index == 5:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B-180 C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[3],angle_current[5], speed)
                elif index == 6:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C-349 F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[3],angle_current[4], speed)
                elif index == 7:
                    gcode = "M21 G90 G01 X160 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 8:
                    gcode = "M21 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 9:
                    gcode = "M21 G90 G01 X{} Y{} Z60 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 10:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A140 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[4],angle_current[5], speed)
                elif index == 11:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B29 C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[3],angle_current[5], speed)
                elif index == 12:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C349 F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[3],angle_current[4], speed)

            elif self.robotA_type == "KT-R4":
                ### 1 2 3 4 5 6 轴角 减  7 8 9 10 11 12 轴角 加  21 复位  21 回零 23 急停
                if index == 1:
                    gcode = "M21 G90 G01 X-130 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 2:
                    gcode = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 3:
                    gcode = "M21 G90 G01 X{} Y{} Z-10 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 4:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[4],angle_current[5], speed)
                elif index == 7:
                    gcode = "M21 G90 G01 X180 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 8:
                    gcode = "M21 G90 G01 X{} Y45 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 9:
                    gcode = "M21 G90 G01 X{} Y{} Z40 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 10:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[4],angle_current[5], speed)
                
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
            

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def jointControRobotRelease(self,index):
        try:
            # gcode = "o117\r\n"
            gcode = "!\r\n"
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
            time.sleep(0.5)
            gcode = f"%\n"
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def robotA_coordinateTeach(self):
        try:
            coordinate_PushbuttonA = {
                self.pushButton_C1min: 1,
                self.pushButton_C2min: 2,
                self.pushButton_C3min: 3,
                self.pushButton_C4min: 4,
                self.pushButton_C5min: 5,
                self.pushButton_C6min: 6,
                self.pushButton_C1add: 7,
                self.pushButton_C2add: 8,
                self.pushButton_C3add: 9,
                self.pushButton_C4add: 10,
                self.pushButton_C5add: 11,
                self.pushButton_C6add: 12,

            }

            for butt, page_ind in coordinate_PushbuttonA.items():
                # butt.clicked.connect(lambda checked, index=page_ind: self.coordControRobotA(index))
                # 连接 pressed 信号到第一个函数
                butt.pressed.connect(lambda checked=False, index=page_ind: self.coordControRobotA(index))
                # 连接 released 信号到第二个函数
                butt.released.connect(lambda checked=False, index=page_ind: self.coordControRobotRelease(index))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def coordControRobotA(self,index):
        try:
            speed = self.speed
            s_data = server.vsrserver.robot_control.status_global
            angle_current = s_data["angle"]
            angle_current = [int(i) for i in angle_current]
            ### 1 2 3 4 5 6 笛卡尔 减  7 8 9 10 11 12 笛卡尔 加
            if index == 1:
                gcode = f"M20 G91 G01 X-{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
            elif index == 2:
                gcode = f"M20 G91 G01 X0 Y-{self.step} Z0 A0 B0 C00 F{speed}\n"
            elif index == 3:
                gcode = f"M20 G91 G01 X0 Y0 Z-{self.step} A0 B0 C00 F{speed}\n"
            elif index == 4:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A-{self.step} B0 C00 F{speed}\n"
            elif index == 5:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B-{self.step} C00 F{speed}\n"
            elif index == 6:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C-{self.step} F{speed}\n"
            elif index == 7:
                gcode = f"M20 G91 G01 X{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
            elif index == 8:
                gcode = f"M20 G91 G01 X0 Y{self.step} Z0 A0 B0 C00 F{speed}\n"
            elif index == 9:
                gcode = f"M20 G91 G01 X0 Y0 Z{self.step} A0 B0 C00 F{speed}\n"
            elif index == 10:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A{self.step} B0 C00 F{speed}\n"
            elif index == 11:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B{self.step} C00 F{speed}\n"
            elif index == 12:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C{self.step} F{speed}\n"            
            
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def coordControRobotRelease(self,index):
        try:
            # gcode = "o117\r\n"
            gcode = "!\r\n"
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
            time.sleep(0.5)
            gcode = f"%\n"
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    ###-- 6. B机器人控制：robotB_jointTeach轴角   robotB_coordinateTeach笛卡尔示教
    def robotB_jointTeach(self):
        try:
            joint_PushbuttonB = {
                self.pushButton_J1min_B: 1,
                self.pushButton_J2min_B: 2,
                self.pushButton_J3min_B: 3,
                self.pushButton_J4min_B: 4,
                self.pushButton_J5min_B: 5,
                self.pushButton_J6min_B: 6,
                self.pushButton_J1add_B: 7,
                self.pushButton_J2add_B: 8,
                self.pushButton_J3add_B: 9,
                self.pushButton_J4add_B: 10,
                self.pushButton_J5add_B: 11,
                self.pushButton_J6add_B: 12,
            }
            for butt, page_ind in joint_PushbuttonB.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.jointControRobotB(index))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def jointControRobotB(self,index):
        try:
            speed = 2000
            s_data_ = server.vsrserver.robot_control.status_global_
            angle_current = s_data_["angle"]
            angle_current = [int(i) for i in angle_current]
            ### 1 2 3 4 5 6 轴角 减  7 8 9 10 11 12 轴角 加
            if index == 1:
                gcode = "M21 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 2:
                gcode = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 3:
                gcode = "M21 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 4:
                gcode = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[4],angle_current[5], speed)
            elif index == 5:
                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B-180 C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[3],angle_current[5], speed)
            elif index == 6:
                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C-349 F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[3],angle_current[4], speed)
            elif index == 7:
                gcode = "M21 G90 G01 X100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 8:
                gcode = "M21 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 9:
                gcode = "M21 G90 G01 X{} Y{} Z120 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 10:
                gcode = "M21 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[4],angle_current[5], speed)
            elif index == 11:
                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B29 C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[3],angle_current[5], speed)
            elif index == 12:
                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C349 F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[3],angle_current[4], speed)

            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 2)
            

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def robotB_coordinateTeach(self):
        try:
            coordinate_PushbuttonB = {
                self.pushButton_C1min_B: 1,
                self.pushButton_C2min_B: 2,
                self.pushButton_C3min_B: 3,
                self.pushButton_C4min_B: 4,
                self.pushButton_C5min_B: 5,
                self.pushButton_C6min_B: 6,
                self.pushButton_C1add_B: 7,
                self.pushButton_C2add_B: 8,
                self.pushButton_C3add_B: 9,
                self.pushButton_C4add_B: 10,
                self.pushButton_C5add_B: 11,
                self.pushButton_C6add_B: 12,
            }

            for butt, page_ind in coordinate_PushbuttonB.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.coordControRobotB(index))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def coordControRobotB(self,index):
        try:
            speed = self.speed
            s_data_ = server.vsrserver.robot_control.status_global_
            angle_current = s_data_["angle"]
            angle_current = [int(i) for i in angle_current]
            ### 1 2 3 4 5 6 笛卡尔 减  7 8 9 10 11 12 笛卡尔 加
            if index == 1:
                gcode = f"M20 G91 G01 X-{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
            elif index == 2:
                gcode = f"M20 G91 G01 X0 Y-{self.step} Z0 A0 B0 C00 F{speed}\n"
            elif index == 3:
                gcode = f"M20 G91 G01 X0 Y0 Z-{self.step} A0 B0 C00 F{speed}\n"
            elif index == 4:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A-{self.step} B0 C00 F{speed}\n"
            elif index == 5:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B-{self.step} C00 F{speed}\n"
            elif index == 6:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C-{self.step} F{speed}\n"
            elif index == 7:
                gcode = f"M20 G91 G01 X{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
            elif index == 8:
                gcode = f"M20 G91 G01 X0 Y{self.step} Z0 A0 B0 C00 F{speed}\n"
            elif index == 9:
                gcode = f"M20 G91 G01 X0 Y0 Z{self.step} A0 B0 C00 F{speed}\n"
            elif index == 10:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A{self.step} B0 C00 F{speed}\n"
            elif index == 11:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B{self.step} C00 F{speed}\n"
            elif index == 12:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C{self.step} F{speed}\n"            
            
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 2)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    ###-- 7. 复位 急停 回零（命令机械臂）
    def cmd_robot(self):
        try:
            cmd_pushButton = {
                self.pushButton_robotHome:21,
                self.pushButton_robotZero:22,
                self.pushButton_robotStop:23,
            }

            cmd_pushButtonB = {
                self.pushButton_robotHome_B:31,
                self.pushButton_robotZero_B:32,
                self.pushButton_robotStop_B:33,
            }
            for butt, page_ind in cmd_pushButton.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.cmd_robotControl(index))
            
            for butt, page_ind in cmd_pushButtonB.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.cmd_robotControlB(index))
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def cmd_robotControl(self,index):
        try:
                if index == 21:
                    gcode = "$h\r\n"
                elif index == 22:
                    gcode = 'M21 G90 G00 X0 Y0 Z0 A0 B0 C0 F2000\r\n'
                elif index == 23:
                    gcode = "o117\r\n"

                server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)

        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    def cmd_robotControlB(self,index):
        try:
                if index == 31:
                    gcode = "$h\r\n"
                elif index == 32:
                    gcode = 'M21 G90 G00 X0 Y0 Z0 A0 B0 C0 F2000\r\n'
                elif index == 33:
                    gcode = "o117\r\n"

                server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 2)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

####### 手动控制界面------End


##############################
####### 状态监控界面------Start
    #### 按钮初始化
    def mointor_pushButton(self):
        try:

            buttons = {
                self.pushButton_bool: 0,
                self.pushButton_int: 1,
                self.pushButton_float: 2,
                self.pushButton_pose: 3,
            }

            for button, page_index in buttons.items():
                button.clicked.connect(lambda checked, index=page_index: self.stackedWidget_mointor.setCurrentIndex(index))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    ### 创建-初始化数据监控表格
    def createMonitorTables(self, table_index, row_count_per_table, var_prefix, modbus_addr_prefix, value_type):
        try:
            table_widget = QTableWidget(self)
            table_widget.setColumnCount(4)
            table_widget.setHorizontalHeaderLabels(["No.", "变量名", "Modbus地址", "值"])
            table_widget.setRowCount(row_count_per_table)

            header_font = QFont("Arial", 18, QFont.Bold)
            table_widget.horizontalHeader().setFont(header_font)
            # table_widget.horizontalHeader().setSectionResizeMode(QHeaderView.Stretch)

            header = table_widget.horizontalHeader()
            header.setSectionResizeMode(0, QHeaderView.ResizeToContents)
            header.setSectionResizeMode(1, QHeaderView.ResizeToContents)
            header.setSectionResizeMode(2, QHeaderView.Stretch)
            header.setSectionResizeMode(3, QHeaderView.ResizeToContents)

            start_index = table_index * row_count_per_table + 1
            for row in range(row_count_per_table):
                index = start_index + row - 1

                # 第一列：No.
                item_no = QTableWidgetItem(str(index + 1))
                item_no.setFont(QFont("Arial", 16))
                item_no.setTextAlignment(Qt.AlignCenter)
                table_widget.setItem(row, 0, item_no)

                # 第二列：变量名
                var_name = f"{var_prefix}{index + 1:02d}"
                item_var_name = QTableWidgetItem(var_name)
                item_var_name.setFont(QFont("Arial", 16))
                item_var_name.setTextAlignment(Qt.AlignCenter)
                table_widget.setItem(row, 1, item_var_name)

                # 第三列：Modbus地址
                if value_type == 'bool':
                    modbus_addr = f"{modbus_addr_prefix}_{index:04d}" 
                elif value_type == 'int':
                    base_addr = 300 if modbus_addr_prefix == "4X" else 0
                    modbus_addr = f"{modbus_addr_prefix}_{base_addr + index:04d}"
                elif value_type in ['float', 'pose']:
                    base_addr = 400 if value_type == 'float' else 500 
                    addr_offset = index * 2
                    modbus_addr = f"{modbus_addr_prefix}_{base_addr + addr_offset:04d}"
                else:
                    modbus_addr = ""
                item_modbus_addr = QTableWidgetItem(modbus_addr)
                item_modbus_addr.setFont(QFont("Arial", 16))
                item_modbus_addr.setTextAlignment(Qt.AlignCenter)
                table_widget.setItem(row, 2, item_modbus_addr)
                
                # 第四列：Modbus地址对应的值
                if value_type == 'bool':
                    value = str(True)
                elif value_type == 'int':
                    value = "0"
                elif value_type == 'float':
                    value = "00.00"
                elif value_type == 'pose':
                    value = "00.00"

                item_value = QTableWidgetItem(value)
                item_value.setFont(QFont("Arial", 16))
                item_value.setTextAlignment(Qt.AlignCenter)
                table_widget.setItem(row, 3, item_value)

            table_widget.resizeColumnsToContents()
            table_widget.resizeRowsToContents()

            size_policy = QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
            size_policy.setHorizontalStretch(1)
            table_widget.setSizePolicy(size_policy)

            return table_widget
        
        except Exception as e:
            robot_logger.error(e, exc_info=True)

    ### 监控--并用于更新数据
    def monitor_(self, page, var_prefix, modbus_addr_prefix, value_type):
        try:
            main_layout = QVBoxLayout(page)
            table_layout = QHBoxLayout()

            row_count_per_table = 14
            table_count = 3

            for table_index in range(table_count):
                table_widget = self.createMonitorTables(table_index, row_count_per_table, var_prefix, modbus_addr_prefix, value_type)
                self.tables.append(table_widget)
                table_layout.addWidget(table_widget)

            main_layout.addLayout(table_layout)
            main_layout.setStretch(0, 1)
            page.setLayout(main_layout)
            page.show()
            self._timerUpdateModbusValue(modbus_addr_prefix)

        except Exception as e:
            robot_logger.error(e, exc_info=True)
    
    ### 设置定时器以定期更新值(500ms/次)
    def _timerUpdateModbusValue(self, modbus_addr_prefix):
        modbus_addr_prefix = ["0X", "4X"]
        self.timer = QTimer()
        self.timer.timeout.connect(lambda: self.updateModbus_values(modbus_addr_prefix))
        self.timer.start(500) 

    ### 监听寄存器值--用于更新值
    def updateModbus_values(self, modbus_addr_prefix):
        try:
            data = self.readModbusRegisters(modbus_addr_prefix)
            for table in self.tables:
                for row in range(table.rowCount()):
                    modbus_addr_item = table.item(row, 2)
                    if modbus_addr_item is not None:
                        modbus_addr = modbus_addr_item.text()
                        if modbus_addr in data:
                            value = data[modbus_addr]
                            
                            ## 根据值类型格式化显示
                            if isinstance(value, bool):
                                if value == 0:
                                    display_value = "False"
                                else:
                                    display_value = "True"
                                # display_value = str(value)
                            elif isinstance(value, int):
                                display_value = str(value)
                            elif isinstance(value, float):
                                display_value = f"{value:.2f}"
                            else:
                                display_value = str(value)
                            
                            value_item = QTableWidgetItem(display_value)
                            value_item.setFont(QFont("Arial", 16))
                            value_item.setTextAlignment(Qt.AlignCenter)
                            table.setItem(row, 3, value_item)

        except Exception as e:
            robot_logger.error(e, exc_info=True)

    ### 读取 Modbus 寄存器
    def readModbusRegisters(self, prefixes):
        try:
            data = {}
            for prefix in prefixes:
                if prefix == "0X":
                    # 读取线圈（布尔值）
                    response = process_master.master_.execute(1, cst.READ_COILS, starting_address=0, quantity_of_x=42)
                    data.update({f"{prefix}_{i:04d}": bool(response[i]) for i in range(len(response))})
                elif prefix == "4X":
                    value_tmp = {
                        'int': 300,
                        'float': 400,
                        'pose': 500
                    }
                    quantities = {
                        'int': 42,
                        'float': 82,
                        'pose': 82
                    }

                    for var_type, start_addr in value_tmp.items():
                        response = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=start_addr, quantity_of_x=quantities[var_type])
                        
                        if var_type == 'int':
                            data.update({f"{prefix}_{start_addr + i:04d}": response[i] for i in range(len(response))})
                        elif var_type in ['float', 'pose']:
                            for i in range(0, len(response), 2):  # 浮点数和位姿占用两个寄存器
                                high_word = response[i]
                                low_word = response[i + 1]
                                float_value = struct.unpack('>f', struct.pack('>HH', high_word, low_word))[0]
                                data[f"{prefix}_{start_addr + i:04d}"] = float_value
            return data

        except Exception as e:
            robot_logger.error(e, exc_info=True)


    ###-- 1. Bool型变量监控
    def boolMonitor(self):
        self.monitor_(self.page_bool, "GB", "0X", 'bool')

    ###-- 2. Int型变量监控
    def intMonitor(self):
        self.monitor_(self.page_int, "GI", "4X", 'int')

    ###-- 3. Float型变量监控
    def floatMonitor(self):
        self.monitor_(self.page_float, "GF", "4X", 'float')

    ###-- 4. Pose型变量监控
    def poseMonitor(self):
        self.monitor_(self.page_pose, "GP", "4X", 'pose')


####### 状态监控界面------End


##############################
####### 机器视觉界面------Start







####### 机器视觉界面------End



##############################
####### 人机交互界面------Start







####### 人机交互界面------End


if __name__ == '__main__':

    process_master = process_Master()
    rtu_master = rtu_Master()
    server = Controler_Server()
    app = QApplication(sys.argv)
    ex = MainWindow()
    ex.show()
    sys.exit(app.exec_())
    server.dorun()

    # ser_thread = Thread(target = server.dorun())
    # ser_thread.start() 



